#!/usr/bin/env python
# -*- coding: UTF-8 -*-

# from scripts.mqtt_client import Mqtt_Client
from scripts.mqttt_client_dw import Mqtt_Client
# from tool import User_Data_Server

import time
import json
import rospy
import os
import tf
import math
import commands
from threading import Timer
import sys
reload(sys)
sys.setdefaultencoding('utf-8')

from sensor_msgs.msg import LaserScan
from nav_msgs.msg import OccupancyGrid,MapMetaData
# from cln_msgs.msg import MapStatus, CleanTask, CleanTaskQueue,StringArray

from cln_msgs.msg import SensorState, CleanDeviceState, HardwareState
from cln_msgs.msg import HardwareStateType, CleanDeviceType, RobotSn
# from cln_msgs.msg import LogMsg, AlarmMsg,DiagnoseFaults
from cln_msgs.msg import LogMsg,DiagnoseFaults
from v3_msgs.msg import *
from cln_msgs.msg import MapStatus, StringArray
from visualization_msgs.msg import MarkerArray
from std_msgs.msg import String, Bool,Int8
from geometry_msgs.msg import PoseStamped
from tool import error_parse
import sys

from tool.log_parser import Log_Parser
log = Log_Parser()


class Realtime_Publication_Node(Mqtt_Client):

    def __init__(self):
        self.__node_name = "ros_app_mqtt_node_1"
        self.__processID = os.getpid()
        self.__robot_safe = False
        self.__in_creating_map = False
        self.__robot_lock_state = 0 # 1 锁机，0 解锁 20230519--dw.liu
        self.__bing_elevator_state = False  # 默认没有绑定 20230626-dw.liu
        self.__sw55_pad_state = 0 # 数据是空的情况，空就当放下 0处理
        self.utils_ctrl_mode = 0 # 清扫外设控制模式 20230723-dw.liu 
        self.__map_name_update = '' # 局部地图更新时候的名字 20231123-dw.liu
        self.__task_list_len = 0 # 实时任务队列长度 20240109-dw.liu
        self.__main_brush_state = 8 #主刷的状态，8默认关闭，21、22、23低中高 20240116-dw.liu
        # self.__data_server = User_Data_Server()
        # 上次发布话题的话题名 时间
        self.data_filter = {}
        # 地图相关的信息
        self.map_data = None
        self.__relocation_state = None

        ##任务信息
        # self.task = CleanTask()  #当前任务信息
        self.task = TaskSingle()  #当前任务信息
        # self.task_queue = CleanTaskQueue()  #记录实时任务队列
        self.task_queue = OnlineTask()  #记录实时任务队列

        ## Mqtt_Client初始化
        Mqtt_Client.__init__(self, self.__node_name)
        self.InitNode()

    def InitNode(self):

        ## ROS初始化
        rospy.init_node(self.__node_name, disable_signals=True)
        rospy.on_shutdown(self.Shutdown)

        self.tfListener = tf.TransformListener()
        self.__robot_type = rospy.get_param("robot_type_sw","SW55A_AA")    # 机器型号
        # if self.__robot_type== 'SW80P_AC':
        #     topic_scan="/scan_up"
        #     laser_frame="/lidar_2d_config/lidar_2d_02/frame_id"
        # else:
        #     topic_scan="/scan"
        #     laser_frame="/lidar_2d_config/lidar_2d_01/frame_id"
        # if self.__robot_type== 'SW80P_AA' or self.__robot_type== 'SW80P_AB' or self.__robot_type== 'SW80P_AC' or self.__robot_type== 'SW80P_AD' or self.__robot_type== 'SW80P_AE':
        topic_scan="/scan"
        laser_frame="/lidar_2d_config/lidar_2d_01/frame_id"
        topic_scan_up="/scan_up"
        laser_frame_up="/lidar_2d_config/lidar_2d_02/frame_id"
        # else:
        #     topic_scan="/scan"
        #     laser_frame="/lidar_2d_config/lidar_2d_01/frame_id"
        ## ROS param
        self.__map_frame = rospy.get_param('/map_frame', "/map")
        self.__base_frame = rospy.get_param('/base_frame', "/base_link")
        self.__laser_frame = rospy.get_param(laser_frame, "laser")
        self.__camera_frame = rospy.get_param('/camera_frame', "/camera_link")
        self.__sw55_pad_state = rospy.get_param('/sw55_pad_state', 0)  # 数据是空的情况，空就当放下 0处理 
       
        self.__disinfect_r_enable = rospy.get_param("/disinfect_r/enable", False) # 右消杀是否使能

        self.__last_robot_state_publish_time = 0. #上次机器人状态消息发布时间戳，单位：s
        self.__robot_state_publish_interval = rospy.get_param('robot_state_publish_interval', 1.0)

        self.__last_robot_state_param_time = 0. #上次获取机器人状态参数的时间戳，单位：s
        self.__robot_state_param_interval = rospy.get_param('robot_state_param_interval', 5.0)

        self.__monitor_mqtt_client_time = 0. #上次监测mqtt连接状态时间戳，单位：s
        self.__monitor_mqtt_client_interval = rospy.get_param('monitor_mqtt_client_interval', 5.0)  # 监测mqtt连接状态

      
        ## 发布：mqtt服务器宕机报警
        # self.__alarm_event_pub = rospy.Publisher("/alarm_event", AlarmMsg, queue_size=1)
        self.__alarm_event_v3_pub = rospy.Publisher("/alarm_event_v3", AlarmMsg, queue_size=1)
        ## 订阅Topic
        ### 锁机状态
        rospy.Subscriber("/robot_param_change", StringArray, callback=self.RobotParamChange_Callback)
        ### 地图相关
        rospy.Subscriber("/map_name", String, callback=self.MapName_Callback)
        # rospy.Subscriber("/map",
        #                  OccupancyGrid,
        #                  self.Map_Callback,
        #                  queue_size=1)
        
        # rospy.Subscriber("/map_metadata",MapMetaData,self.Map_Callback,queue_size=1)
        rospy.Subscriber("/ax_map_metadata",MapMetaData,self.Map_Callback,queue_size=1)
        
        rospy.Subscriber("/map_status",
                         MapStatus,
                         self.MapStaus_Callback,
                         queue_size=1)
        rospy.Subscriber("/tracked_pose",
                         PoseStamped,
                         self.TrackedPose_Callback,
                         queue_size=1)
        rospy.Subscriber("/trajectory_node_list",
                         MarkerArray,
                         self.TrajectoryList_Callback,
                         queue_size=1)

        ### 外设传感器相关
        rospy.Subscriber(topic_scan,
                         LaserScan,
                         self.LaserScan_Callback,
                         queue_size=1)
        rospy.Subscriber(topic_scan_up,
                         LaserScan,
                         self.LaserScanUp_Callback,
                         queue_size=1)
        rospy.Subscriber("/depth2laser",
                         LaserScan,
                         self.DepthCameraScan_Callback,
                         queue_size=1)
        rospy.Subscriber("/sensors_state",
                         SensorState,
                         self.SensorsState_Callback,
                         queue_size=1)
        rospy.Subscriber("/devices_state",
                         CleanDeviceState,
                         self.CleanDeviceState_Callback,
                         queue_size=1)
        rospy.Subscriber("/hardware_state",
                         HardwareState,
                         self.HardwareState_Callback,
                         queue_size=1)

        # 订阅外设控制模式数据，然后通过外设话题发布出去 20230724-dw.liu
        rospy.Subscriber("/utils_ctrl_mode",
                         Int8,
                         self.UtilsCtrlMode_Callback,
                         queue_size=1)

        ### 任务相关
        # rospy.Subscriber("/task_echo",
        #                  CleanTask,
        #                  self.ExecutingTask_Callback,
        #                  queue_size=1)
        # rospy.Subscriber("/task_queue",
        #                  CleanTaskQueue,
        #                  self.RealtimeTaskQueue_Callback,
        #                  queue_size=1)

        ### 机器人相关
        rospy.Subscriber("/robot_safe",
                         Bool,
                         self.RobotSafe_Callback,
                         queue_size=10)
        ### 机器人运行过程中实时错误相关
        rospy.Subscriber("/diagnose_faults",
                         DiagnoseFaults,
                         self.RobotError_Callback,
                         queue_size=10)

        ### 订阅机器人是否绑定电梯 20230626-dw.liu
        rospy.Subscriber("/cloudPlatform_bindElevator",Bool,self.BindElevator_Callback,queue_size=1)
        ## 局部地图更新的时候，发布正在更新的地图名
        rospy.Subscriber("/map_name_update",String,callback=self.MapNameUpdate_Callback)
        rospy.Subscriber("/relocation_state",
                         RelocationState,
                         self.RelocationState_Callback,
                         queue_size=1)

        ## 任务相关 20240905 
        rospy.Subscriber("/online_task_single", TaskSingle, self.ExecutingTask_Callback, queue_size=1)
        rospy.Subscriber("/online_tasks_deque", OnlineTask, self.RealtimeTaskQueue_Callback, queue_size=1)
        
        # 监测mqtt客户端连接状态
        self.MonitorMqttClient()

    def RelocationState_Callback(self, msg):
        self.__relocation_state = msg
          

    #-----------------------------------#
    #******#  ROS topic Callback #******#
    #-----------------------------------#

    def RobotParamChange_Callback(self,msg = StringArray()):
        '''
        锁机状态
        '''
        print(msg.data)
        for param_one in msg.data:
            print("---------RobotParamChange_Callback------------")
            if param_one == "/robot_lock_state":
                
                self.__robot_lock_state = rospy.get_param('/robot_lock_state',0)

            if param_one == "/sw55_pad_state":
                
                self.__sw55_pad_state = rospy.get_param('/sw55_pad_state', 0)
            

    '''
    清扫外设控制模式
    '''
    def UtilsCtrlMode_Callback(self,msg):
        self.utils_ctrl_mode = msg.data
        #print(self.utils_ctrl_mode)



    """ 
        当前地图名称回调函数
    """
    @error_parse.try_catch
    def MapName_Callback(self, msg_data):
        self.__map_name = msg_data.data
        pub_msg = {}
        pub_msg["name"] = self.__map_name
        self.dataAppendPlus(pub_msg)
        self.PublishTopic("map_name", json.dumps(pub_msg), qos=1)
        rospy.loginfo("[{}]Map name: {}".format(self.__node_name,
                                                self.__map_name))

    """ 
        地图回调函数
    """
    @error_parse.try_catch
    def Map_Callback(self, msg_data):
        self.map_data = msg_data

    """ 
        建图状态回调函数
    """
    @error_parse.try_catch
    def MapStaus_Callback(self, msg_data):
        self.__in_creating_map = msg_data.in_creating
        # rospy.loginfo("[{}]Map creating status: {}".format(
            # self.__node_name, self.__in_creating_map))

    """ 
        机器人位姿回调函数
    """
    @error_parse.try_catch
    def TrackedPose_Callback(self, msg_data):
        if not self.checkSendTopip("TrackedPose_Callback", 0.5):
            return
            # 时间间隔超过0.5s发送，2hz
        if not self.map_data:
            return
        pub_data = {}
        self.__pose_x = msg_data.pose.position.x
        self.__pose_y = msg_data.pose.position.y
        rot = [
            msg_data.pose.orientation.x, msg_data.pose.orientation.y,
            msg_data.pose.orientation.z, msg_data.pose.orientation.w
        ]
        (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(rot)
        self.__pose_yaw = yaw

        pub_data["pose_x"] = self.__pose_x
        pub_data["pose_y"] = self.__pose_y
        pub_data["pose_yaw"] = self.__pose_yaw
        # pub_data["origin_x"] = self.map_data.info.origin.position.x
        # pub_data["origin_y"] = self.map_data.info.origin.position.y
        pub_data["origin_x"] = self.map_data.origin.position.x
        pub_data["origin_y"] = self.map_data.origin.position.y
        self.dataAppendPlus(pub_data)

        # 获取前向雷达数据
        pub_data["front_distance"] = 0.0
        scan_msg = rospy.wait_for_message("/scan", LaserScan, timeout=1)
        if scan_msg:
            # 获取当前机型
            robot_type_sw = rospy.get_param("/robot_type_sw", "SW55A_AA")
            if robot_type_sw == "SW55A_AA" or robot_type_sw == "SW80P_AA" or robot_type_sw == "SW80P_AC" or robot_type_sw == "SW80P_AE":
                # 获取当前雷达距离
                index = int((0 + math.pi - scan_msg.angle_min) / scan_msg.angle_increment)
                if scan_msg.ranges[index] > scan_msg.range_max:
                    pub_data["front_distance"] = scan_msg.range_max
                else:
                    pub_data["front_distance"] = scan_msg.ranges[index]
            if robot_type_sw == "SW80P_AB" or robot_type_sw == "SW80P_AD" or robot_type_sw == "SW55A_AB":
                # 获取当前雷达距离
                index = int((0 - scan_msg.angle_min) / scan_msg.angle_increment)
                if scan_msg.ranges[index] > scan_msg.range_max:
                    pub_data["front_distance"] = scan_msg.range_max
                else:
                    pub_data["front_distance"] = scan_msg.ranges[index]

        print("[TrackedPose_Callback]: ", pub_data)
        self.PublishTopic("robot_pose", json.dumps(pub_data), qos=0)

    """ 
        轨迹列表回调函数
    """
    @error_parse.try_catch
    def TrajectoryList_Callback(self, msg_data):
        if not self.checkSendTopip("TrajectoryList_Callback", 1.5):
            return
        if self.__in_creating_map:
            trajectory_list = self.TrajectoryList_list(msg_data)
            if trajectory_list:
                self.dataAppendPlus(trajectory_list)
                self.PublishTopic("map_add_track",
                                json.dumps(trajectory_list),
                                qos=0)

    """ 
        雷达数据回调函数
    """
    @error_parse.try_catch
    def LaserScan_Callback(self, msg_data):
        # if self.__robot_type== 'SW80P_AA' or self.__robot_type== 'SW80P_AB' or self.__robot_type== 'SW80P_AC' or self.__robot_type== 'SW80P_AD' or self.__robot_type== 'SW80P_AE':
        #     return
        if not self.checkSendTopip("LaserScan_Callback", 0.5):
            return
        sacn_data = self.GetLaserScan(msg_data, self.__laser_frame, 160)
        if sacn_data:
            self.PublishTopic("radar_info", json.dumps(sacn_data), qos=0)
    
    def LaserScanUp_Callback(self, msg_data):
        if not self.checkSendTopip("LaserScanUp_Callback", 0.5):
            return
        sacn_data = self.GetLaserScanUp(msg_data, 160)
        if sacn_data:
            self.PublishTopic("multi_lidar_info", json.dumps(sacn_data), qos=0)

    """ 
        深度相机数据回调函数
    """
    @error_parse.try_catch
    def DepthCameraScan_Callback(self, msg_data):
        if not self.checkSendTopip("DepthCameraScan_Callback", 1):
            return
        sacn_data = self.GetLaserScan(msg_data, self.__base_frame, 160)
        # 深度相机失效时，发布空数据
        # if sacn_data:
        self.PublishTopic("depth_camera", json.dumps(sacn_data), qos=0)

    """ 
        传感器状态回调函数
    """
    @error_parse.try_catch
    def SensorsState_Callback(self, msg_data):
        if not self.checkSendTopip("SensorsState_Callback", 1):
            return
        sensor_state = msg_data

        # 发布 /sensor_values 消息, 包含传感器状态数据消息
        sensor_values = []
        sensor_values += self.ConvertSensors(sensor_state.mul_sonar)
        sensor_values += self.ConvertSensors(sensor_state.mul_bumper)
        sensor_values += self.ConvertSensors(sensor_state.mul_drop)
        pub_sensor_value = {"items": sensor_values}
        self.PublishTopic("sensor_values", json.dumps(pub_sensor_value), qos=0)

    """ 
        清扫设备状态回调函数
    """
    @error_parse.try_catch
    def CleanDeviceState_Callback(self, msg_data):
        if not self.checkSendTopip("CleanDeviceState_Callback", 0.5):
            return
        # 发布 /sensor_values 消息, 包含传感器状态数据消息
        clean_device_values = []
        # 刷盘设备
    
        clean_device_values.append(self.ConvertCleanDevice(msg_data.main_brush))
        # 记录主刷状态变化
        if self.__main_brush_state != msg_data.main_brush.state.state:
            log_msg = "[topic:/devices_state] [main_brush id :{}]from <last state: {}>  to <now state: {}> .".format(msg_data.main_brush.device_id.value,self.__main_brush_state,msg_data.main_brush.state.state)
            log.LogInfo(self.__node_name,log_msg)
            self.__main_brush_state = msg_data.main_brush.state.state

        clean_device_values.append(self.ConvertCleanDevice(msg_data.side_brush))
        # clean_device_values.append(self.ConvertCleanDevice(msg_data.sidebrush_lf))
        # clean_device_values.append(self.ConvertCleanDevice(msg_data.sidebrush_rf))
        # clean_device_values.append(self.ConvertCleanDevice(msg_data.sidebrush_lb))
        # clean_device_values.append(self.ConvertCleanDevice(msg_data.sidebrush_rb))

        # 电机设备

        clean_device_values.append(self.ConvertCleanDevice(msg_data.lift_mainbrush))
        clean_device_values.append(self.ConvertCleanDevice(msg_data.lift_sidebrush))
        # clean_device_values.append(self.ConvertCleanDevice(msg_data.lift_lf))
        # clean_device_values.append(self.ConvertCleanDevice(msg_data.lift_rf))

        clean_device_values.append(self.ConvertCleanDevice(msg_data.vacuumfan))

        # 吸水扒设备
        clean_device_values.append(self.ConvertCleanDevice(msg_data.main_absorbent))

        clean_device_values.append(self.ConvertCleanDevice(msg_data.absorbent_lf))

        clean_device_values.append(self.ConvertCleanDevice(msg_data.absorbent_rf))

        # 消杀设备

        clean_device_values.append(self.ConvertCleanDevice(msg_data.disinfect_left))

        clean_device_values.append(self.ConvertCleanDevice(msg_data.disinfect_right))


        # 箱体设备
        clean_device_values.append(self.ConvertCleanDevice(msg_data.box_cw))

        clean_device_values.append(self.ConvertCleanDevice(msg_data.disinfect))

        clean_device_values.append(self.ConvertCleanDevice(msg_data.box_dw))

        clean_device_values.append(self.ConvertCleanDevice(msg_data.box_rubbish))

        clean_device_values.append(self.ConvertCleanDevice(msg_data.box_dust))

        # 其它状态

        clean_device_values.append(self.ConvertCleanDevice(msg_data.hand_rail))

        clean_device_values.append(self.ConvertCleanDevice(msg_data.carpet_mode))
        
        clean_device_values.append(self.ConvertCleanDevice(msg_data.uv_led))

        # 清扫外设控制模式  
        device_dict = {
            "device_id":201, #所有外设 CleanDeviceType().ALL_DEVICE 201 
            "state":self.utils_ctrl_mode,
            "health_state": 0,  #健康程度(小于20应当更换)
            "percentage": 0.0  #剩余容量 百分比形式
        }
        clean_device_values.append(device_dict)

        pub_sensor_value = {"items": clean_device_values}
        self.dataAppendPlus(pub_sensor_value)
        self.PublishTopic("devices_state", json.dumps(pub_sensor_value), qos=0)

    """ 
        硬件状态回调函数
    """
    @error_parse.try_catch
    def HardwareState_Callback(self, msg_data):
        if not self.checkSendTopip("HardwareState_Callback", 0.5):
            return
        hardware_state = msg_data

        language = rospy.get_param("/language",0) #0 中文 1 英文 2 繁体中文 3 西班牙语 4 法语 5 日语 6 韩语 7 德语 
        cleandevice_config_default_params = rospy.get_param("/cleandevice_config_default",{})
        # 切换语言
        translation_list = cleandevice_config_default_params["translation"]
        clean_device_mode = hardware_state.clean_device_mode.decode('utf-8')
        clean_device_mode_key = ""
        if clean_device_mode: 
            if translation_list["mode_1"] and clean_device_mode in translation_list["mode_1"]:
                clean_device_mode_key = "mode_1"
            if translation_list["mode_2"] and clean_device_mode in translation_list["mode_2"]:
                clean_device_mode_key = "mode_2"
            if translation_list["mode_4"] and clean_device_mode in translation_list["mode_4"]:
                clean_device_mode_key = "mode_4"
            

        # 发布 /hardware_state 消息
        hardware_state = {
            "Version": hardware_state.Version,  #硬件版本号
            "humidity": hardware_state.humidity,  #环境潮湿度
            "temp_env": hardware_state.temp_env,  #环境温度
            "sound_dB": hardware_state.sound_dB,  #环境噪音(工作噪音)
            "bat_state": hardware_state.bat_state,  #电池状态 充放电状态:0放电 1充电
            "hand_charge_state": hardware_state.charge_line, # 手动充电状态 20230731-dw.liu
            "electrode_state": hardware_state.charge_ctrl,  #充电口状态
            "bat_voltage": hardware_state.bat_voltage,  #电压,
            "bat_current": hardware_state.bat_current,  #放电电流(充电为负)
            "bat_percentage": hardware_state.bat_percentage,  #0~100
            "bat_description": hardware_state.bat_description,  #电池文字
            "bat_temp": hardware_state.Version,  #电池温度(预留)
            "manual":
            self.ConvertBool(hardware_state.manual
                             ),  #手动控制清扫模式(False代表机器驱动,True代表无动力,需要手动推行)
            "clamshell_state": self.ConvertBool(hardware_state.clamshell_state),#翻盖状态   0 放下­  1 上翻(检修模式)
            # "e_stop": self.ConvertBool(
            #     hardware_state.e_stop),  #急停状态是否开启,True为开启急停,False为关闭急停(正常运动)
            "e_stop":hardware_state.e_stop,  #急停模式修改 0为关闭急停(正常运动)，1为开启急停,2开启暂停 20230802-dw.liu
            "l_encoder": hardware_state.l_encoder,  #
            "r_encoder": hardware_state.r_encoder,  #
            "l_wheel_curr": hardware_state.l_wheel_curr,  #
            "r_wheel_curr": hardware_state.r_wheel_curr,  #
            "disinfect_l_liquid": hardware_state.disinfect_l_liquid,  #左侧消杀设备液位值
            "disinfect_r_liquid": hardware_state.disinfect_r_liquid,  #
            # ft.wang 20240624 添加清扫模式
            "clean_device_mode": hardware_state.clean_device_mode,      
            "clean_device_mode_key": clean_device_mode_key  #暂只支持mode_1,mode_2,mode_4, 用于多国语言显示
        }

        hardware_state["ipc_current_time"] = self.GetIpcCurrentTime()
        # hardware_state["ipc_current_time"] = time.strftime('%Y-%m-%d %H:%M:%S %Z', time.localtime()) # 添加时区 20240219-dw.liu
        
        self.dataAppendPlus(hardware_state)
        self.PublishTopic("hardware_state", json.dumps(hardware_state), qos=0)

    """ 
        正在执行的任务数据回调函数
    """
    @error_parse.try_catch
    def ExecutingTask_Callback(self, task):
        #print("task.type = {}, task.task_name= {},task.state = {}".format(task.type,task.task_name,task.state))
        self.task = task
        #任务完成，发布任务完成报告/report_last 消息
        # if task.state == CleanTask.FINISHED:
        # print("[ExecutingTask_Callback] status: {}".format(task.task_echo.task_status))

        if task.task_echo.task_status.status == TaskState.FINISHED:
            self.PublishReportLast(task)
        if not self.checkSendTopip("ExecutingTask_Callback", 0.5):
            return
        # self.PublishRobotState()  # 20230904,出现topic:task_echo有时没有数据，导致话题robot_state发布失败
    
    @error_parse.try_catch
    def RealtimeTaskQueue_Callback(self, data):
        """ 
            实时执行任务队列回调函数
        """
        self.task_queue = data

    """ 
       机器人安全确认信息回调函数
    """
    
    @error_parse.try_catch
    def RobotSafe_Callback(self, safe):
        self.__robot_safe = safe.data

    """
    机器人运行过程中实时错误回调函数
    """
    @error_parse.try_catch
    def RobotError_Callback(self, msg_data):
        self.robotCodeList = {}
        for alarm_event in msg_data.alarms:
            __log_level = alarm_event.event_level
            __log_code = alarm_event.event_code
            __cancel_trigger = alarm_event.cancel_trigger
            __event_data = alarm_event.event_data
            if error_parse.ignoreCode(__log_code):
                continue
            # __log_code=getErrorCode(__log_code)
                # rospy.logerr("__log_code {}".format(__log_code))
            if __log_level > 1 and __log_code > 0:
                msg = "code:{} codeHex:{} data:{} msg:{}".format(
                    __log_code,
                    '%#x' % __log_code,
                    __cancel_trigger,
                    alarm_event.event_data)
                # rospy.logerr(msg)
                # print_file(msg)
                if not __cancel_trigger:
                    self.robotCodeList[__log_code] = __event_data
                # map 转list
        list_ = []
        keys_ = self.robotCodeList.keys()
        for item in keys_:
            list_.append(
                {"key": item, "value": 1, "event_data":self.robotCodeList[item]})
                # 发送出去
        self.PublishTopic("robot_error",
                        json.dumps(list_),
                        qos=1,
                        retain=True)
    
    def BindElevator_Callback(self,msg):
        '''
        机器人是否绑定电梯20230626-dw.liu
        '''
        self.__bing_elevator_state = msg.data

    def MapNameUpdate_Callback(self,msg):
        '''
        局部地图更新的时候，发布正在更新的地图名
        '''
        self.__map_name_update = msg.data
        

    def checkSendTopip(self, topic, interval=1):
        """_summary_
        true 可以发送 false 不可发送
        Args:
            topic (_type_): 话题的名称
            interval (_type_): 间隔秒数
        """
        if not self.Connected():
            return False
        if topic not in self.data_filter.keys():
            self.data_filter[topic] = 0
        if (time.time() - self.data_filter[topic]) > interval:
            self.data_filter[topic] = time.time()
            return True
        return False
    
    def GetIpcCurrentTime(self):
        ''''
        获取机器当前时间包含时区
        '''
        try:
            ipc_current_time = ""

            command = 'date +"%Y-%m-%dT%H:%M:%S%:z"'
            status,output = commands.getstatusoutput(command)
            # print("status :{}".format(status))
            # print("output :{}".format(output))

            if status != 0:
                raise Exception("date  command error : {}".format(output))

            ipc_current_time = output

        except Exception as error:

            logmsg = ("[{}][GetIpcCurrentTime] date command error : {}".format(self.__node_name,str(error)))  # 记载状态
            log.LogInfo(self.__node_name,logmsg)

        return ipc_current_time

    def isDockering(self):
        """_summary_
        true 正在充电的过程中 false
        Returns:
            _type_: _description_
        """
        if not self.task:
            return False
        # if(rospy.time.now() - self.task.stamp).to_sec() > 2.0:     #新增超时判断
        #     return False
        if self.task.task_base.task_type.type == self.task.task_base.task_type.CHARGING_TASK:
            return True
        return False

    def isP2P(self):
        """_summary_
        true 正在导航过程中 false
        Returns:
            _type_: _description_
        """
        if not self.task:
            return False
        # if(rospy.time.now() - self.task.stamp).to_sec() > 2.0:     #新增超时判断
        #     return False
        if self.IsRobotFree():
            return False
        # if (self.task.task_base.task_name == 'p2p_move') and (self.task.task_base.task_type.type
        #                                                 == self.task.task_base.task_type.P2P_TASK):
        #     # if self.task.state == CleanTask.FINISHED or self.task.state == CleanTask.TERMINATED:
        #     if self.task.task_echo.task_status.status == TaskState.FINISHED or self.task.task_echo.task_status.status == TaskState.TERMINATED:
        #         return False
        #     else:
        #         return True
        if self.task.task_base.task_type.type == self.task.task_base.task_type.P2P_TASK:
            return True
        return False

    def IsRobotFree(self):
        '''
            判断任务队列是否为空
        '''
        return len(self.task_queue.taskgroup_list) == 0
    
    def IsRobotTaskListChange(self):
        '''
            判断任务队列是否变化
        '''
        if self.__task_list_len != len(self.task_queue.taskgroup_list) :
            self.__task_list_len = len(self.task_queue.taskgroup_list) 
            return True
        else:
            return False

    '''
        转换传感器/硬件在线状态到数值
    '''

    def ConvertOnline(self, state):
        return 1 if (state == HardwareStateType().ENABLE) else 0

    '''
        转换传感器消息
    '''

    def ConvertSensor(self, sensor_msg):
        sensor_dict = {
            "sensor_id": sensor_msg.id.value,
            "sensor_type": sensor_msg.type.value,
            "x": sensor_msg.positions.x,
            "y": sensor_msg.positions.y,
            "yaw": sensor_msg.positions.theta,
            "enable": self.ConvertOnline(sensor_msg.online.state)
        }
        if sensor_msg.type.value == sensor_msg.type.SONAR:
            sensor_dict["value"] = sensor_msg.value
        else:
            sensor_dict["value"] = self.ConvertBool(sensor_msg.activated)
        return sensor_dict

    '''
        转换传感器数组消息
    '''

    def ConvertSensors(self, sensors_msg):
        result = []
        for sensor_msg in sensors_msg:
            result.append(self.ConvertSensor(sensor_msg))
        return result

    '''
        转换外设消息
    '''

    def ConvertCleanDevice(self, device_msg):
        device_dict = {
            "device_id": device_msg.device_id.value,
            "state": device_msg.state.state,
            "health_state": device_msg.health_state,  #健康程度(小于20应当更换)
            "percentage": device_msg.percentage  #剩余容量 百分比形式
        }
        return device_dict

    '''
        获得tf坐标变换
    '''

    def GetTf(self, target_frame, source_frame):
        for i in range(3):
            try:
                (trans, rot) = self.tfListener.lookupTransform(
                    target_frame, source_frame, rospy.Time(0.0))
                (roll, pitch,
                 yaw) = tf.transformations.euler_from_quaternion(rot)
                return True, trans[0], trans[1], yaw
            except Exception as err:
                rospy.logwarn("[{}]Failed to get TF<{} -> {}>: {}]".format(
                    self.__node_name, source_frame, target_frame, err))
                time.sleep(0.2)
                pass
        return False, 0, 0, 0

    @error_parse.try_catch
    def GetLaserScan(self, data, frame_name, max_laser_scan_count=80):
        """获得LaserScan数据

        Args:
            data (_type_): _description_
            frame_name (_type_): _description_
            max_laser_scan_count (int, optional): _description_. Defaults to 80.

        Raises:
            Exception: _description_

        Returns:
            _type_: _description_
        """
        if not data:
            return []
        try:
            state, x, y, yaw = self.GetTf(self.__map_frame, frame_name)
            if not state: 
                raise Exception("Get TF error!")
            ##根据数据长度设置过滤条件，防止数据过大
            laser_data_len = len(data.ranges)
            laser_data = []
            interval = 1
            last_index = -100
            if laser_data_len > max_laser_scan_count:
                interval = int(laser_data_len / max_laser_scan_count)

            ##将数据从极坐标转换到/map坐标系
            for i in range(laser_data_len):
                if (data.ranges[i] != float("inf")) and (data.ranges[i] != 0.0):
                    if (i - last_index) > interval:
                        laser_angle = data.angle_min + i * data.angle_increment
                        #laser_x = int((x - self.map_originx + data.ranges[i]*(math.cos(laser_angle+yaw))) / self.resolution)
                        #laser_y = self.map_height - int((y - self.map_originy + data.ranges[i]*(math.sin(laser_angle+yaw))) / self.resolution)

                        laser_x = x + data.ranges[i] * (math.cos(laser_angle +
                                                                    yaw))
                        laser_y = y + data.ranges[i] * (math.sin(laser_angle +
                                                                    yaw))

                        laser_pose = {"x": laser_x, "y": laser_y}
                        laser_data.append(laser_pose)
                        last_index = i
            return laser_data

        except Exception as err:
            rospy.logwarn("[{}]Failed to get laser data<{}>: {}".format(
                self.__node_name, frame_name, err))
            return []
    

    @error_parse.try_catch
    def GetLaserScanUp(self, data_up, max_laser_scan_count=80):
        """获得LaserScan数据
        Args:
            data (_type_): _description_
            frame_name (_type_): _description_
            max_laser_scan_count (int, optional): _description_. Defaults to 80.
        Raises:
            Exception: _description_
        Returns:
            _type_: _description_
        """
        if not data_up:
            return []
        try:
            data = rospy.wait_for_message("/scan", LaserScan, timeout=0.5)
            if not data:
                return []
            state_up, x_up, y_up, yaw_up = self.GetTf(self.__map_frame, "laser_up")
            state, x, y, yaw = self.GetTf(self.__map_frame, "laser")

            if not state_up: 
                raise Exception("Get UP TF error!")
            if not state: 
                raise Exception("Get TF error!")
            
            ##根据数据长度设置过滤条件，防止数据过大
            laser_data_len_up = len(data_up.ranges)
            laser_data_len = len(data.ranges)

            laser_data = {
                "scan": [],
                "scan_up": []
            }

            interval_up = 1
            interval = 1

            last_index_up = -100
            last_index = -100

            if laser_data_len_up > max_laser_scan_count:
                interval_up = int(laser_data_len_up / max_laser_scan_count)
            if laser_data_len > max_laser_scan_count:
                interval = int(laser_data_len / max_laser_scan_count)

            ##将数据从极坐标转换到/map坐标系
            for i in range(laser_data_len_up):
                if (data_up.ranges[i] != float("inf")) and (data_up.ranges[i] != 0.0):
                    if (i - last_index_up) > interval_up:
                        laser_angle_up = data_up.angle_min + i * data_up.angle_increment

                        laser_x_up = x_up + data_up.ranges[i] * (math.cos(laser_angle_up +
                                                                    yaw_up))
                        laser_y_up = y_up + data_up.ranges[i] * (math.sin(laser_angle_up +
                                                                    yaw_up))

                        laser_pose_up = {"x": laser_x_up, "y": laser_y_up}
                        laser_data["scan_up"].append(laser_pose_up)
                        last_index_up = i
            
            for i in range(laser_data_len):
                if (data.ranges[i] != float("inf")) and (data.ranges[i] != 0.0):
                    if (i - last_index) > interval:
                        laser_angle = data.angle_min + i * data.angle_increment

                        laser_x = x + data.ranges[i] * (math.cos(laser_angle +
                                                                    yaw))
                        laser_y = y + data.ranges[i] * (math.sin(laser_angle +
                                                                    yaw))

                        laser_pose = {"x": laser_x, "y": laser_y}
                        laser_data["scan"].append(laser_pose)
                        last_index = i
            
            return laser_data

        except Exception as err:
            rospy.logwarn("[{}]Failed to get laser data: {}".format(
                self.__node_name, err))
            return []

    def TrajectoryList_list(self, data):
        """ 
        建图轨迹 数据处理
        只保留 两点间距离大1m的数据
        最多只保留80个
        Args:
            data (_type_): _description_

        Returns:
            _type_: _description_
        """
        points = []
        last_x = 0.
        last_y = 0.
        count_ = 0
        max_point_count = 60
        ## 过滤建图轨迹点，防止数据量过大
        for marker in data.markers:
            count_ += len(marker.points)
        step_ = 1
        if count_ > max_point_count:
            step_ = int(count_ / max_point_count) + 1
        for marker in data.markers:
            #不能用range step 否则取出的数会超过60的，最大可以有会为119
            for i in range(len(marker.points)):
                if (i % step_ > 0):
                    continue
                point = marker.points[i]
                if (abs(point.x - last_x) + abs(point.y - last_y)) > 1:
                    # 距离超过1m的才会为示
                    ret_point = {}
                    ret_point["x"] = point.x
                    ret_point["y"] = point.y
                    last_x = point.x
                    last_y = point.y
                    points.append(ret_point)
        ret = {"points": points}
        return ret

    
    '''
        发布机器人任务状态消息
    '''

    def PublishRobotState(self):
        pub_msg = {}

        if not self.IsRobotFree():
            print(self.task.task_echo.task_name)
            pub_msg["task_name"] = self.task.task_echo.task_name
            pub_msg["task_id"] = self.task.task_echo.task_id
            pub_msg["route_percentage"] = self.task.task_echo.progress
            pub_msg["total_percentage"] = self.task.task_echo.progress  #其他节点完善后修改
            pub_msg["estm_remain_time"] = self.task.task_base.estimated_time
        else:
            pub_msg["task_name"] = ""
            pub_msg["task_id"] = 0
            pub_msg["route_percentage"] = 0
            pub_msg["total_percentage"] = 0
            pub_msg["estm_remain_time"] = 0

        print("self.task.task_echo.task_status.status: ", self.task.task_echo.task_status.status)
        pause_state = 2  #默认暂停不可用
        # if self.task.state == CleanTask.PAUSED:
        if self.task.task_echo.task_status.status == TaskState.PAUSE:
            pause_state = 1  #已处于暂停状态
        # elif self.task.state == CleanTask.EXECUTING:
        elif self.task.task_echo.task_status.status == TaskState.EXECUING:
            pause_state = 0  #暂停可用
        # print("pause_state: ", pause_state)
        if self.task.task_echo.task_status.status == TaskState.USER_CANCEL: # 禁止取消，默认允许被取消 20231016-dw.liu
            pub_msg["stop_state"] = 2
        else:
            pub_msg["stop_state"] = 0 
        
        pub_msg["task_list_len"] = self.__task_list_len

        if self.IsRobotTaskListChange():
            # 实时任务队列是否发生变化 1 有变化 0没变化
            pub_msg['task_list_change'] = 1
        else:
            pub_msg['task_list_change'] = 0

        pub_msg["ipc_current_time"] = self.GetIpcCurrentTime()
        # pub_msg["ipc_current_time"] = time.strftime('%Y-%m-%d %H:%M:%S %Z', time.localtime()) # 添加时区 20240219-dw.liu

        pub_msg["pause_state"] = pause_state
        print("isDockering: ", self.isDockering())
        pub_msg["gohome_state"] = self.ConvertBool(self.isDockering())
        pub_msg["point_state"] = self.ConvertBool(self.isP2P())
        pub_msg["location_is"] = self.ConvertBool(self.__robot_safe)
        pub_msg["robot_lock_state"] = self.__robot_lock_state
        pub_msg["sw55_pad_state"] = self.__sw55_pad_state                     # 操作杆抬起状态 20230705-dw.liu
        pub_msg["robot_bind_elevator"] = self.ConvertBool(self.__bing_elevator_state) # 机器人是否绑定电梯 20230626-dw.liu
        pub_msg["relocation_start"] = 0  #预留
        pub_msg["is_map_building_enabled"] = self.ConvertBool(
            self.__in_creating_map)
        pub_msg["map_name_update"] = self.__map_name_update # 地图更新时候的名字
        pub_msg["barcode_relocation_state"] = 0
        if self.__relocation_state:
            if self.__relocation_state.option == RelocationState.REFLECTOR:
                pub_msg["barcode_relocation_state"] = int(self.__relocation_state.state)
            # 超时则自动清空
            time_interval = (rospy.Time.now() - self.__relocation_state.stamp).to_sec()
            if time_interval > 3.0:
                pub_msg["barcode_relocation_state"] = 0

        print(pub_msg)
        self.dataAppendPlus(pub_msg)
        self.PublishTopic("robot_state", json.dumps(pub_msg), qos=0)

    '''
        发布任务完成消息
    '''

    def PublishReportLast(self, task):
        pub_msg = {}
        pub_msg["mapName"] = task.map_name
        pub_msg["taskName"] = task.task_name
        pub_msg["taskID"] = int(task.id)
        pub_msg["taskType"] = task.clean_mode
        pub_msg["startTime"] = self.ConvertTime(task.start_time.to_sec())
        pub_msg["endTime"] = self.ConvertTime(task.end_time.to_sec())
        # 任务报告中的清扫面积改成   理论清扫面积*清扫率
        pub_msg["clean_area"] = str(task.theoretic_cov_rate * task.cov_rate)
        pub_msg["coverage_rate"] = str(task.executed)
        pub_msg["end_error_code"] = task.error_code
        pub_msg["end_error_msg"] = task.error_info
        self.dataAppendPlus(pub_msg)
        self.PublishTopic("report_last", json.dumps(pub_msg), qos=1)

    
    def RealtimePublishRobotState(self):
        rate = rospy.Rate(100) #设置100赫兹
        while (not rospy.is_shutdown()) :
            
            if (time.time() - self.__last_robot_state_publish_time) > self.__robot_state_publish_interval:
                # 指定间隔时间发布/robot_state消息
                self.PublishRobotState()
                self.__last_robot_state_publish_time = time.time()
            rate.sleep()

    def Shutdown(self):
        '''
        节点关闭操作
        '''
        rospy.logwarn("[{}]Stopping the node...".format(self.__node_name))
        os.system("kill -9 %s" % self.__processID)

    def StartRunning(self):
        """ 
        节点启动函数
        """
        while (not rospy.is_shutdown()):
            if self.Connected() == False:
                self.Shutdown()
            time.sleep(3)


    def PublishEmqxAlarmMsgV3(self, cancel_trigger):
        alarm_msg = AlarmMsg()
        alarm_msg.event_code = 0X0221
        alarm_msg.node_name = self.__node_name
        self.__alarm_event_v3_pub.publish()


    # def PublishEmqxAlarmMsg(self,cancel_trigger):
    #     '''
    #         mqtt服务器宕机报警
    #     '''
    #     language_type = rospy.get_param("/language", 0)#0 中文 1 英文 2 繁体中文 3 西班牙语 4 法语 5 日语 6 韩语 7 德语 
    #     alarm_msg = AlarmMsg()
    #     alarm_msg.node_name = self.__node_name
    #     alarm_msg.event_time = rospy.Time.now()
    #     # print(rospy.Time.now())
    #     if cancel_trigger == False:
    #         alarm_msg.event_code = 0X0331
    #         alarm_msg.event_level = AlarmMsg().LEVEL_3
    #         if language_type == 0:
    #             alarm_msg.event_data = "MQTT服务器宕机"
    #             alarm_msg.event_code_str = "MQTT服务器宕机"
    #         elif language_type == 1:
    #             alarm_msg.event_data = "MQTT server is down"
    #             alarm_msg.event_code_str = "MQTT server is down"
    #         elif language_type == 4:
    #             alarm_msg.event_data = "MQTT server is down"
    #             alarm_msg.event_code_str = "Le serveur MQTT est en  panne"


    #         alarm_msg.cancel_trigger = False

    #     else:
    #         alarm_msg.event_code = 0X0331
    #         alarm_msg.event_level = AlarmMsg().LEVEL_3
    #         if language_type == 0:
    #             alarm_msg.event_data = "MQTT服务器正常"
    #             alarm_msg.event_code_str = "MQTT服务器正常"
    #         elif language_type == 1:
    #             alarm_msg.event_data = "MQTT server is up"
    #             alarm_msg.event_code_str = "MQTT server is up"
    #         elif language_type == 4:
    #             alarm_msg.event_data = "MQTT server is up"
    #             alarm_msg.event_code_str = "Le serveur MQTT est en  bonne"

    #         alarm_msg.cancel_trigger = True

    #     self.__alarm_event_pub.publish(alarm_msg)




    def MonitorMqttClient(self):
        # 监测mqtt客户端运行状态
        rate = rospy.Rate(1) #设置100赫兹

        # 连接mqtt服务
        self.ConnectMqttServer()

        # 重联时长
        reconnec_mqtt_server_time = 0.0 #单位（s）

        while (not rospy.is_shutdown()) :

            if (time.time() - self.__last_robot_state_publish_time) > self.__robot_state_publish_interval:
                # 指定间隔时间发布/robot_state消息
                self.PublishRobotState()
                self.__last_robot_state_publish_time = time.time()
            
            if (time.time() - self.__last_robot_state_param_time) > self.__robot_state_param_interval:
                # 指定间隔获取机器锁机状态
                self.__robot_lock_state = rospy.get_param('/robot_lock_state',0)
                self.__sw55_pad_state = rospy.get_param('/sw55_pad_state', 0)
                self.__last_robot_state_param_time = time.time()
       

            if (time.time() - self.__monitor_mqtt_client_time) > self.__monitor_mqtt_client_interval:
                # 监测mqtt连接状态
                # print("self.Connected():{}".format(self.Connected()))
                if self.Connected() == False:
                    
                    reconnec_mqtt_server_time = reconnec_mqtt_server_time + self.__monitor_mqtt_client_interval
                    # 断开连接，重联
                    # print("断开连接，重联")
                    self.ReconnectMqttServer()
                else:
                    reconnec_mqtt_server_time  = 0

                self.__monitor_mqtt_client_time = time.time()

            # time.sleep(60)# 测试
            # kick_current_mqtt_client_ret = self.KickCurrentMqttClient(self.GetCurrentClient_ID())
            # if kick_current_mqtt_client_ret == True:
            #     self.CreateNewMqttClient()
            
            if reconnec_mqtt_server_time >= 30:
                # 重联时间超过1分钟
                # 获取 emqx 服务器状态
                emqx_status_ret = self.GetEmqxStatus()
                if emqx_status_ret == False:
                    # 发送服务器宕机报警信息
                    # self.PublishEmqxAlarmMsg(False)
                    PublishEmqxAlarmMsgV3(False)
                    # 重启emqx服务器
                    reconnect_emqx_server_ret =  self.ReconnectEmqxServer()
                    # emqx 服务器 正常
                    if reconnect_emqx_server_ret == True:
                        # 解除报警
                        #  self.PublishEmqxAlarmMsg(True)
                        PublishEmqxAlarmMsgV3(True)
                else:
                    # emqx 服务器 正常,但客户端连接状态失败，重联不起作用(断链了，或者发送消息失败了)
                    # 杀死当前客户端，新建客户端连接
                    kick_current_mqtt_client_ret = self.KickCurrentMqttClient(self.GetCurrentClient_ID())
                    if kick_current_mqtt_client_ret == True:
                        self.CreateNewMqttClient()
                        
                reconnec_mqtt_server_time = 0   # 每隔一分钟，重联一次

            rate.sleep()



def print_file_init():
    path_ = "/home/aoting/user_config/realtime.txt"
    if os.path.exists(path_):
        os.remove(path_)
    print_file("realtime init")


def print_file(arg):
    time_ = time.strftime('%Y-%m-%d %H:%M:%S', time.localtime(time.time()))
    with open("/home/aoting/user_config/realtime.txt", "a") as file:
        file.write(time_ + " " + arg + "\n")


if __name__ == '__main__':
    node = Realtime_Publication_Node()
    print_file_init()
    print_file("mqtt connect ...")
    # node.ConnectMqttServer()                              # 更改之前
    # node.RealtimePublishRobotState()              # 更改之前

    # rospy.on_shutdown(node.Shutdown)
    # node.StartRunning()

    # node.MonitorMqttClient()

    rospy.spin()
    
    
    
    
    
    
    
    
    
    
    
    
    
'''
def CleanDeviceState_Callback(self, msg_data):
        if not self.checkSendTopip("CleanDeviceState_Callback", 0.5):
            return
        # 发布 /sensor_values 消息, 包含传感器状态数据消息
        clean_device_values = []
        # 刷盘设备
        # if self.__data_server.IsUsedDevice(
        #         msg_data.main_brush.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.main_brush))
        # # 记录主刷状态变化
        # if self.__main_brush_state != msg_data.main_brush.state.state:
        #     log_msg = "[topic:/devices_state] [main_brush id :{}]from <last state: {}>  to <now state: {}> .".format(msg_data.main_brush.device_id.value,self.__main_brush_state,msg_data.main_brush.state.state)
        #     log.LogInfo(self.__node_name,log_msg)
        #     self.__main_brush_state = msg_data.main_brush.state.state


        # if self.__data_server.IsUsedDevice(
        #         msg_data.sidebrush_lf.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.sidebrush_lf))
        # if self.__data_server.IsUsedDevice(
        #         msg_data.sidebrush_rf.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.sidebrush_rf))
        # if self.__data_server.IsUsedDevice(
        #         msg_data.sidebrush_lb.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.sidebrush_lb))
        # if self.__data_server.IsUsedDevice(
        #         msg_data.sidebrush_rb.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.sidebrush_rb))

        # # 电机设备
        # if self.__data_server.IsUsedDevice(
        #         msg_data.lift_mainbrush.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.lift_mainbrush))
        # if self.__data_server.IsUsedDevice(msg_data.lift_lf.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.lift_lf))
        # if self.__data_server.IsUsedDevice(msg_data.lift_rf.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.lift_rf))
        # if self.__data_server.IsUsedDevice(msg_data.vacuumfan.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.vacuumfan))

        # # 吸水扒设备
        # if self.__data_server.IsUsedDevice(
        #         msg_data.main_absorbent.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.main_absorbent))
        # if self.__data_server.IsUsedDevice(
        #         msg_data.absorbent_lf.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.absorbent_lf))
        # if self.__data_server.IsUsedDevice(
        #         msg_data.absorbent_rf.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.absorbent_rf))

        # # 消杀设备
        # # print(msg_data.disinfect_left)
        # # if self.__data_server.IsUsedDevice(
        # #         msg_data.disinfect_left.device_id.value):
        # if "Spray" in self.__robot_type:    # 机器喷雾
        #     clean_device_values.append(
        #             self.ConvertCleanDevice(msg_data.disinfect_left))
        #     # if self.__data_server.IsUsedDevice(
        #     #         msg_data.disinfect_right.device_id.value):
        #     # KT60 没有右消杀，屏蔽，方法与安道龙保持一致：根据消杀是否使能 20231114-dw.liu
        #     # if not self.__disinfect_r_enable:
        #     if "KT60" not in self.__robot_type:# KT60单喷雾，其他SW55双喷雾
        #         clean_device_values.append(
        #                     self.ConvertCleanDevice(msg_data.disinfect_right))
        #     # print(clean_device_values)

        # # 箱体设备
        # if self.__data_server.IsUsedDevice(msg_data.box_cw.device_id.value):
        #     clean_device_values.append(self.ConvertCleanDevice(
        #         msg_data.box_cw))
        # if self.__data_server.IsUsedDevice(msg_data.disinfect.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.disinfect))
        # if self.__data_server.IsUsedDevice(msg_data.box_dw.device_id.value):
        #     clean_device_values.append(self.ConvertCleanDevice(
        #         msg_data.box_dw))
        # if self.__data_server.IsUsedDevice(
        #         msg_data.box_rubbish.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.box_rubbish))
        # if self.__data_server.IsUsedDevice(msg_data.box_dust.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.box_dust))

        # # 其它状态
        # if self.__data_server.IsUsedDevice(msg_data.hand_rail.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.hand_rail))
        # if self.__data_server.IsUsedDevice(
        #         msg_data.carpet_mode.device_id.value):
        #     clean_device_values.append(
        #         self.ConvertCleanDevice(msg_data.carpet_mode))

        # 清扫外设控制模式  
        device_dict = {
            "device_id":201, #所有外设 CleanDeviceType().ALL_DEVICE 201 
            "state":self.utils_ctrl_mode,
            "health_state": 0,  #健康程度(小于20应当更换)
            "percentage": 0.0  #剩余容量 百分比形式
        }
        clean_device_values.append(device_dict)

        pub_sensor_value = {"items": clean_device_values}
        self.dataAppendPlus(pub_sensor_value)
        self.PublishTopic("devices_state", json.dumps(pub_sensor_value), qos=0)
'''
